﻿#include <stdexcept>
#include <string>

#include "HermiteFrag.hpp"

namespace MMF
{
    namespace BSpline
    {

        HermiteFrag::HermiteFrag()
        {
        }

        HermiteFrag::HermiteFrag(float startPos, const std::vector<TesVector3> &pts, const std::vector<TesVector3> &vs)
            : mStartPos(startPos), mLength(0.0f), mPoints(pts), mVelocity(vs)
        {
            if (pts.size() != 2 || pts.size() != vs.size())
                throw std::length_error((__FILE__ + std::to_string(__LINE__)).c_str());
            mLength = (mPoints[1] - mPoints[0]).Length();
        }

        TesVector3 HermiteFrag::GetPoint(float distance)
        {
            TesVector3 ret = {};

            // 剩余长度
            float delatLen = distance - mStartPos;

            // 计算t
            float t = delatLen / mLength;

            // 拟合
            ret = mPoints[0] * (1 - 3 * t * t + 2 * t * t * t) +
                  mVelocity[0] * (t - 2 * t * t + t * t * t) +
                  mPoints[1] * (-(t * t) + t * t * t) +
                  mVelocity[1] * (3 * t * t - 2 * t * t * t);

            return ret;
        }

        bool HermiteFrag::isIn(float distance)
        {
            if (distance >= mStartPos && distance <= mStartPos + mLength)
                return true;

            return false;
        }

    }
}
